#define _GNU_SOURCE
#include <sched.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <time.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include "osal_time.h"
#include "gpio_if.h"

#include <semaphore.h>
#include <pthread.h>

#include "private.h"
#include "motorControl.h"
#include "e22Control.h"
#include "crc.h"

#define BIG_LITTLE_SWAP16(x)        ( (((*(short int *)&x) & 0xff00) >> 8) | \
                                      (((*(short int *)&x) & 0x00ff) << 8) )
 
#define BIG_LITTLE_SWAP32(x)        ( (((*(long int *)&x) & 0xff000000) >> 24) | \
                                      (((*(long int *)&x) & 0x00ff0000) >> 8) | \
                                      (((*(long int *)&x) & 0x0000ff00) << 8) | \
                                      (((*(long int *)&x) & 0x000000ff) << 24) )
#define uint32_t  unsigned int

typedef struct mesg{
    unsigned char control;    //控制字段
    unsigned short int param; //参数
    unsigned char reserve;     //保留
}Mesg;

pthread_t ThreadE22; //控制板之间通信
// sem_t commandSem; //
unsigned char controlBuf[8] = {0};

static int e22RUN = 1;
static void *ThreadE22Recv(void *arg);
static int32_t ReturnIrqHandler(uint16_t gpio, void *data);
static int32_t StopIrqHandler(uint16_t gpio, void *data);

MotorControl* motorCtrl = NULL;

int pipefd[2];
E22Control* e22Ctrl = NULL;

int main(int argc, char *argv[]) 
{
    int ret = 0;
    unsigned short int param = 0;
    unsigned char control = 0;
    unsigned char e22Mesg[4] = {0};

   
    //初始化管道pipe 0：读取 1：写入
    pipe(pipefd);

    //crc初始化
    crcInit();
    //初始化心通信信号量
    // sem_init(&commandSem, 0, 0);

    //初始化电机控制器
    motorCtrl = motorInit();
    if(motorCtrl == NULL)
    {
        printf("motorInit error\n");
        return 0;
    }

    //初始化e22控制器
    e22Ctrl = e22ControlInit();

    //创建E22接收线程
    pthread_create(&ThreadE22, NULL, ThreadE22Recv, (void*)e22Ctrl);
    //左侧传感器操作
    pinSetInterrupt(LEFT_SWITCH, NEGATIVE_EDGE, ReturnIrqHandler, (void*)motorCtrl);
    //右侧传感器操作
    pinSetInterrupt(RIGHT_SWITCH, NEGATIVE_EDGE, StopIrqHandler, (void*)motorCtrl);
    //重置电机状态 停止，速度为1200
    motorSetModel(motorCtrl, 5);
    motorSetSpeed(motorCtrl, 1200);

    while (TRUE)
    {
        // sem_wait(&commandSem);
        read(pipefd[0], e22Mesg, 4);
        param = e22Mesg[1] << 8 | e22Mesg[2];
        control = e22Mesg[0];
        printf("%2x %4x\n", control, param);
        switch (control)
        {
            case 0x10://控制电机模式
                // if(pinReadValue(GPIO_I(18) == 1) && param == 1){
                //     param = 2;
                // }
                motorSetModel(motorCtrl, param);
                break;
            case 0x11://控制电机速度
                motorSetSpeed(motorCtrl, param);
                break;
            case 0x12://读取电机模式
                reportedModel(e22Ctrl, motorCtrl);
                break;
            case 0x13://读取电机速度
                reportedSpeed(e22Ctrl, motorCtrl);
                break;
            case 0xFF://停止点击运动
                motorSetModel(motorCtrl, 5);
                break;
            default:
                break;
        }

        if(control == 0xFF){ 
            e22RUN = 0;
            break;
        }
            
    }


    pthread_join(ThreadE22, NULL);

    printf("release: pipefd\n");
    close(pipefd[0]);
    close(pipefd[1]);

    // ret = sem_destroy(&commandSem);
    // if(ret != 0 ){
    //     printf("error: destroy commandSem");
    //     return -1;
    // }
    printf("release: GPIO\n");
    pinUnsetInterrupt(LEFT_SWITCH);
    pinUnsetInterrupt(RIGHT_SWITCH);
    printf("release: e22\n");
    e22ControlRelease(e22Ctrl);
    printf("release: motor\n");
    motorRelease(motorCtrl);

    return 0;
}

int reportedSpeed(E22Control* e22Ctrl, MotorControl *motorCtrl)
{

    unsigned short int speed = 0;
    unsigned short int crc = 0;
    unsigned char e22Mesg[8] = {MESG_HEAD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF};
    speed = motorGetSpeed(motorCtrl);
    
    e22Mesg[1] = 0x51;
    e22Mesg[2] = (speed & 0xFF00) >> 8;
    e22Mesg[3] = speed & 0xFF;
    // printf("speed :%d %x %x\n", speed, e22Mesg[2], e22Mesg[3]);
    crc = crcCompute(e22Mesg, 5);
    e22Mesg[5] = (crc & 0xFF00) >> 8;
    e22Mesg[6] = (crc & 0xFF);
    e22ControlWrite(e22Ctrl, e22Mesg, 8);
    
}

int reportedModel(E22Control* e22Ctrl, MotorControl *motorCtrl)
{

    unsigned short int model = 0;
    unsigned short int crc = 0;
    unsigned char e22Mesg[8] = {MESG_HEAD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF};
    model = motorGetModel(motorCtrl);
    e22Mesg[1] = 0x50;
    e22Mesg[3] = model;
    crc = crcCompute(e22Mesg, 5);
    e22Mesg[5] = (crc & 0xFF00) >> 8;
    e22Mesg[6] = (crc & 0xFF);
    e22ControlWrite(e22Ctrl, e22Mesg, 8);
    
}

/**
 * 清扫机器人到达尾端需要返回
 */
static int32_t ReturnIrqHandler(uint16_t gpio, void *data)
{
    MotorControl* motorCtrl = (MotorControl*)data;
    GpioDisableIrq(gpio);
    printf("ReturnIrqHandler \n");
    sleep(1);
    motorSetModel(motorCtrl, 5);//停止操作
    sleep(4);
    motorSetModel(motorCtrl, RUN_RIGHT);//反向操作
    return GpioEnableIrq(gpio);
}

int reportedOK(E22Control* e22Ctrl)
{


    unsigned short int crc = 0;
    unsigned char e22Mesg[8] = {0xC2, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF};
    crc = crcCompute(e22Mesg, 5);
    e22Mesg[5] = (crc & 0xFF00) >> 8;
    e22Mesg[6] = (crc & 0xFF);
    e22ControlWrite(e22Ctrl, e22Mesg, 8);
    
}

/**
 * 清扫机器人到达首端需要停止
 */
static int32_t StopIrqHandler(uint16_t gpio, void *data)
{
    MotorControl* motorCtrl = (MotorControl*)data;
    GpioDisableIrq(gpio);
    printf("StopIrqHandler \n");
    sleep(1);
    motorSetModel(motorCtrl, 5);//停止操作
    sleep(1);
    reportedOK(e22Ctrl);
    return GpioEnableIrq(gpio);
}

/**
 * 与换行车进行通信
 */
static void *ThreadE22Recv(void *arg)
{
    int ret = 0;
    E22Control* e22c = (E22Control*)arg;
    e22ControlWrite(e22c, controlBuf, 6);
    printf("ThreadE22Recv\n");
    while (e22RUN)
    {
        ret = e22ControlRead(e22c, controlBuf, 8);
        if (ret == 8){//头尾校验
            if(((controlBuf[5] << 8) | controlBuf[6])  == crcCompute(controlBuf, 5) ){//CRC校验

                if(controlBuf[0] == MESG_HEAD){

                    write(pipefd[1], controlBuf+1, 4);
                }else if(controlBuf[0] == 0xCF)
                {
                    write(pipefd[1], controlBuf+1, 4);
                    break;
                }
                if(controlBuf[1] == 0xFF){ //仅用作调试
                    break;
                }
            }else{
                printf("CRC %x\n", crcCompute(controlBuf, 5));
            }
        } 
    }
    pthread_exit(NULL);
    return NULL;
}